#include <ros/ros.h>
#include <iostream>
#include <std_msgs/Bool.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Eigen>
#include <string>
#include <mavros_msgs/PositionTarget.h>
#include "quadrotor_msgs/PositionCommand.h"

#define TAKEOFF_SUCCESS_THRE 0.1

enum poscmd_server_state
{
    INIT,
    CLIMB,
    READY
};

struct drone_setpoint_raw
{
    Eigen::Vector3d position_;
    Eigen::Vector3d velocity_;
    Eigen::Vector3d acceleration_;
    float yaw_;
    float yaw_rate_;
};

class hls_poscmd_server
{
private:
    // ros utils
    ros::NodeHandle nh_;
    ros::Publisher offboard_pub_, arming_pub_, landing_pub_;
    ros::Publisher setpoint_position_pub_, setpoint_raw_pub_;
    ros::Publisher takeoff_finish_pub_;

    ros::Subscriber pos_sub_;
    ros::Subscriber poscmd_sub_;
    ros::Subscriber takeoffcmd_sub_, landingcmd_sub_;
    ros::Timer hls_poscmd_server_rosTimer_;

    // callbacks
    void RcvPosCallback(const geometry_msgs::PoseStamped &msg);
    void RcvTakeoffCmdCallback(const std_msgs::BoolConstPtr &msg);
    void RcvLandingCmdCallback(const std_msgs::BoolConstPtr &msg);
    /**Poscmd Topic Callback Here**********************************************************************************/
    void RcvPoscmdCallback(const quadrotor_msgs::PositionCommandConstPtr &msg);
    /****************************************************************************************************************/
    void PoscmdServerTimerCallback(const ros::TimerEvent &);

    // util methods
    void setSetpointPosition(double x, double y, double z);
    void setSetpointRaw(double pos_x, double pos_y, double pos_z,
                        double vel_x, double vel_y, double vel_z,
                        double acc_x, double acc_y, double acc_z,
                        float yaw, float yaw_rate);

    void pubSetpointPosition();
    void pubSetpointRaw();

    // params
    bool free_yaw_;             // flag on whether using the planned yaw or not.
    Eigen::Vector3d init_pos_;  // initial pose where the planner starts.

    // data
    bool recieved_takeoffcmd_flag_;
    bool recieved_landingcmd_flag_;

    poscmd_server_state state_;

    Eigen::Vector3d pos_recieved_;

    Eigen::Vector3d setpoint_position_;
    drone_setpoint_raw setpoint_raw_;

public:
    hls_poscmd_server(ros::NodeHandle &nh);
    ~hls_poscmd_server(){};

};